// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GyroCalibration.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Runtime.Perception
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.Serialization;
    using System.Text;
    using System.Threading.Tasks;

    /// <summary>
    /// Class which contains the gyro calibration information.
    /// </summary>
    [DataContract]
    public class GyroCalibration
    {
        /// <summary>
        /// Serialized filename.
        /// </summary>
        public static readonly string Filename = "GyroCalibration.xml";
        
        /// <summary>
        /// Trust gyro drift above a threshold if odometry says we're not turning at all.
        /// This happens when then robot comes to a sudden stop but momentum carries it further.
        /// It also happens when wheel speeds are meant to be equal but the robot actually drifts
        /// ever-so-slightly to one side. Also, sometimes the wheels "settle" into the next stepper
        /// increment after stopping.
        /// </summary>
        /// <remarks>
        /// This is used only when odometry is reporting *zero* motion.
        /// </remarks>
        [DataMember(Name = "DriftThresholdRadiansPerSecond")]
        private double driftThresholdRadiansPerSecond;

        /// <summary>
        /// The magnitude above which to ignore readings during gyro calibration
        /// </summary>
        [DataMember(Name = "GyroCalibrationThreshold")]
        private double gyroCalibrationThreshold;

        /// <summary>
        /// The factor by which gyro Y values have to be scaled.
        /// The gyro default values when integrated result in less than 360 degrees for a 360 degree turn.
        /// </summary>
        [DataMember(Name = "GyroYCalibrationFactor")]
        private double gyroYCalibrationFactor;

        /// <summary>
        /// If odometry says we're turning more than gyro reports, treat as wheel slip.
        /// Threshold for delta between what odometry and gyro are reporting above which we
        /// suspect the wheels are slipping and trust the gyro.
        /// </summary>
        /// <remarks>
        /// This is used only when odometry is reporting *some* motion. Otherwise we assume
        /// the gyro is drifting (or perhaps someone is pushing the robot).
        /// </remarks>
        [DataMember(Name = "SlipThresholdPercentage")]
        private double slipThresholdPercentage;

        /// <summary>
        /// Initializes a new instance of the <see cref="GyroCalibration"/> class.
        /// </summary>
        /// <param name="driftThresholdRadiansPerSecond">Trust gyro drift above this threshold if odometry says we're not turning at all.</param>
        /// <param name="gyroCalibrationThreshold"> The magnitude above which to ignore readings during gyro calibration</param>
        /// <param name="gyroYCalibrationFactor"> The factor by which gyro Y values have to be scaled.</param>
        /// <param name="slipThresholdPercentage">Threshold for delta between what odometry and gyro are reporting above which we trust the gyro.</param>
        public GyroCalibration(double driftThresholdRadiansPerSecond, double gyroCalibrationThreshold, double gyroYCalibrationFactor, double slipThresholdPercentage)
        {
            this.driftThresholdRadiansPerSecond = driftThresholdRadiansPerSecond;
            this.gyroCalibrationThreshold = gyroCalibrationThreshold;
            this.gyroYCalibrationFactor = gyroYCalibrationFactor;
            this.slipThresholdPercentage = slipThresholdPercentage;
        }

        /// <summary>
        /// Gets or sets the DriftThresholdRadiansPerSecond
        /// </summary>
        public double DriftThresholdRadiansPerSecond
        {
            get
            {
                return this.driftThresholdRadiansPerSecond;
            }

            set
            {
                this.driftThresholdRadiansPerSecond = value;
            }    
        }

        /// <summary>
        /// Gets or sets the GyroCalibrationThreshold
        /// </summary>
        public double GyroCalibrationThreshold
        {
            get
            {
                return this.gyroCalibrationThreshold;
            }

            set
            {
                this.gyroCalibrationThreshold = value;
            }
        }

        /// <summary>
        /// Gets or sets the GyroYCalibrationFactor
        /// </summary>
        public double GyroYCalibrationFactor
        {
            get
            {
                return this.gyroYCalibrationFactor;
            }

            set
            {
                this.gyroYCalibrationFactor = value;
            }
        }

        /// <summary>
        /// Gets or sets the SlipThresholdPercentage
        /// </summary>
        public double SlipThresholdPercentage
        {
            get
            {
                return this.slipThresholdPercentage;
            }

            set
            {
                this.slipThresholdPercentage = value;
            }
        }
    }
}
